/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    state_StandUp.cpp
  * @author  Mentos Seetoo
  * @brief   Finite State Machine for standing up.
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
//TODO 使用指令的姿态代替估计的姿态？
#include "state_StandUp.h"
/* Private defines -----------------------------------------------------------*/
using namespace Quadruped;

/* Founctions ----------------------------------------------------------------*/
template <typename T>
FSM_StandUp<T>::FSM_StandUp(GaitScheduler<T> *p_gait,
                            StateEstimator<T> *p_estimator,
                            DesiredCoMState<T> *p_command,
                            Dynamic<T> *p_dynamic,
                            BalanceController<T>  *p_controller,
                            T run_period):FSM_Base<T>::FSM_Base(run_period)
{
    FSM_Base<T>::_name = FSM_StateName::STAND_UP;
    p_BalanceController = p_controller;
    p_Dynamic = p_dynamic;
    p_GaitScheduler = p_gait;
    p_CoMCommand = p_command;
    p_StateEstimator = p_estimator;
}

template <typename T>
void FSM_StandUp<T>::RunState()
{
    /*
        Interpolation for stand up.
    */
    Vec3<T> pNextStep;
    Vec3<T> pDesiredFoot[4] = {Vec3<T>::Zero()};

    for(int leg(0); leg < 4; leg++)
    {
        pNextStep = p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(0, 0,-p_CoMCommand->CoM_height) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);        

        p_GaitScheduler->SetFootPlacement(p_StateEstimator->p_foot_M[leg], pNextStep, 0.0, leg);
        SysLog->Record(_DEBUG_,"Scheduler","Swing Leg %d final point: %f, %f, %f",leg,
                        pNextStep(0), pNextStep(1), pNextStep(2));
        SysLog->Record(_DEBUG_,"Scheduler","Swing Leg %d start point: %f, %f, %f",leg,
                        p_StateEstimator->p_foot_M[leg](0), p_StateEstimator->p_foot_M[leg](1),
                        p_StateEstimator->p_foot_M[leg](2));
    }

    //OPTIMAL gait generate可以修改一下实现形式，把for拿出来，这样上下两个for可以合并
    //!< Generate trajectory in {M}.
    p_GaitScheduler->gaitGenerate();

    //!< set targets in {B}.
    for (uint8_t leg(0); leg < 4; leg++)
    {
        pDesiredFoot[leg] = p_GaitScheduler->GetFootPos(leg);

        p_Dynamic->LegCommand[leg].p = (p_Dynamic->BodyState.rBody_M2B.transpose()
                                     * pDesiredFoot[leg]) - p_Dynamic->getHipLocation(leg);
        
        SysLog->Record(_INFO_,"Scheduler","Leg Command in M-Frame %d: %f, %f, %f",leg,
                        pDesiredFoot[leg](0),
                        pDesiredFoot[leg](1), 
                        pDesiredFoot[leg](2));

        SysLog->Record(_INFO_,"Scheduler","Leg Command in B-Frame %d: %f, %f, %f",leg,
                                    p_Dynamic->LegCommand[leg].p(0), p_Dynamic->LegCommand[leg].p(1), p_Dynamic->LegCommand[leg].p(2));
    }

    //!< Stand up finished and transit to 
    if(p_GaitScheduler->GetExeStanceFlag()==1)
    {
        FSM_Base<T>::SetNextState(FSM_StateName::JUMP);
        //FSM_Base<T>::SetNextState(FSM_StateName::BOUNCE);
        //FSM_Base<T>::SetNextState(FSM_StateName::LOCOMOTION);
    }
}

/**
* @brief  Stand up state can switch to any state(except standup)
*         if stand up motion is finished.
* @return 0: transition done.
*         1: robot standing up, request is suspended.
*         2: error transition request.
* @author Mentos Seetoo
*/
template <typename T>
int FSM_StandUp<T>::Transition(FSM_Base<T>* target_state)
{
    //Standing up
    if(p_GaitScheduler->GetExeStanceFlag()==0)
        return 1;
    
    //Ready to switch
    switch (target_state->StateName())
    {
        case FSM_StateName::PASSIVE:
            p_GaitScheduler->changeGait(GaitType::STAND_UP);
            SysLog->Record(_INFO_,"FSM","State is switched to PASSIVE...");
            return 0;
        case FSM_StateName::LOCOMOTION:
            if(p_GaitScheduler->changeGait(GaitType::WALK))//replaced with TROT CANTER(NOT Optimized)
            {
                p_GaitScheduler->SetCoMVelocity(0.15f, 0.0f, 0.0f);
                //for CANTER
                p_GaitScheduler->SetSampleFactor(0.2);
                p_GaitScheduler->SetInterPolationPorpotion(0.8);
                //p_CoMCommand->SetVelocityCommand(0.1f, 0.0f, 0.0f,0.001f);
                SysLog->Record(_INFO_,"FSM","State is switched to LOCOMOTION...");
                return 0;
            }
            else
                return 1;
        case FSM_StateName::JUMP:
            if(p_GaitScheduler->changeGait(GaitType::JUMP))
            {
                p_CoMCommand->SetBeforeJumpHeightCommand(0.12f);
                p_CoMCommand->SetBoundJumpHeightCommand(0.30f);
                p_CoMCommand->SetAfterJumpHeightCommand(0.22f);//0.25
                p_GaitScheduler->SetExeBoundJumpTime(0.05);//0.05 is constant.
                p_GaitScheduler->SetExeRetractTime(0.10);
                p_GaitScheduler->SetStopExeRetractTime(0.18);
                p_GaitScheduler->SetFallTime(0.30f);
                p_GaitScheduler->SetEndWholeJumpTime(0.38f);
                SysLog->Record(_INFO_,"FSM","State is switched to JUMP...");
                return 0;
            }
            else
                return 1;
        case FSM_StateName::BOUNCE:
            if(p_GaitScheduler->changeGait(GaitType::BOUNCE))
            {
                p_CoMCommand->SetBeforeJumpHeightCommand(0.12f);
                p_CoMCommand->SetBoundJumpHeightCommand(0.30f);
                p_CoMCommand->SetAfterJumpHeightCommand(0.20f);
                p_GaitScheduler->SetExeBoundJumpTime(0.05);//0.05
                p_GaitScheduler->SetExeRetractTime(0.20);
                p_GaitScheduler->SetStopExeRetractTime(0.24);
                p_GaitScheduler->SetFallTime(0.30f);
                SysLog->Record(_INFO_,"FSM","State is switched to JUMP...");
                return 0;
            }
            else
                return 1;
        default:
            SysLog->Record(_ERROR_,"FSM","Error transition in STAND_UP, request is ignored...");
            return 2;
    }
}


template class FSM_StandUp<float>;
template class FSM_StandUp<double>;

/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
